#include "CenterOfMassSensorPlotFactory.h"
#include "CenterOfMassSensorPlot.h"

#include <boost/extension/extension.hpp>

#include <MMM/Motion/Plugin/WholeBodyDynamicPlugin/WholeBodyDynamicSensor.h>

using namespace MMM;

// register this factory
SensorPlotFactory::SubClassRegistry CenterOfMassSensorPlotFactory::registry(PLT_STR(WholeBodyDynamicSensor::TYPE), &CenterOfMassSensorPlotFactory::createInstance);

CenterOfMassSensorPlotFactory::CenterOfMassSensorPlotFactory() : SensorPlotFactory() {}

CenterOfMassSensorPlotFactory::~CenterOfMassSensorPlotFactory() {}

std::string CenterOfMassSensorPlotFactory::getName()
{
    return PLT_STR(WholeBodyDynamicSensor::TYPE);
}

std::string CenterOfMassSensorPlotFactory::getType()
{
    return WholeBodyDynamicSensor::TYPE;
}

std::string CenterOfMassSensorPlotFactory::getUnit() {
    return "Center of mass position (in cm)";
}

std::string CenterOfMassSensorPlotFactory::getValueName() {
    return "Center of mass";
}

SensorPlotPtr CenterOfMassSensorPlotFactory::createSensorPlot(SensorPtr sensor) {
    WholeBodyDynamicSensorPtr s = boost::dynamic_pointer_cast<WholeBodyDynamicSensor>(sensor);
    if (!s) {
        MMM_ERROR << sensor->getType() << " could not be castet to " << WholeBodyDynamicSensor::TYPE << std::endl;
        return nullptr;
    }
    return SensorPlotPtr(new CenterOfMassSensorPlot(s));
}

SensorPlotFactoryPtr CenterOfMassSensorPlotFactory::createInstance(void *)
{
    return SensorPlotFactoryPtr(new CenterOfMassSensorPlotFactory());
}

extern "C"
BOOST_EXTENSION_EXPORT_DECL SensorPlotFactoryPtr getFactory() {
    return SensorPlotFactoryPtr(new CenterOfMassSensorPlotFactory());
}

extern "C"
BOOST_EXTENSION_EXPORT_DECL std::string getVersion() {
    return SensorPlotFactory::VERSION;
}
